#ifndef UHM_UTIL_MAP_HXX_
#define UHM_UTIL_MAP_HXX_

namespace uhm {

  // Map_ store Map_Entry_ with its Id_. 
  // Push back method record the order of Map_Entry_ with its ID_.
  template<class Id_>
  class Map_ : public Disp_ {
  public:
    typedef Id_ _Id_;
    
    class Entry_ : public Disp_ {
    private:
    protected:
      Id_ _id;
      Int_ _idx;
    public:
      Entry_() { }
      Entry_(Id_ id, Int_ idx) : _id(id), _idx(idx) { }
      Entry_(const Entry_ &b) : _id(b._id), _idx(b._idx) { }
      virtual~Entry_() { }

      Bool_ is_equal(Id_ &id) const { return (_id == id); }

      Id_& id() { return _id; }
      Int_& idx() { return _idx; }

      // TODO :: Need separate interface comparable...
      Bool_ operator<(const Entry_ &b) const {  return (this->_id < b._id);  }
      Bool_ operator==(const Entry_ &b) const { return (this->_id == b._id); }
      Bool_ operator!=(const Entry_ &b) const {
        return !((*this) == b);
      }

      // TODO :: Need separate interface assignable(?)
      Entry_& operator=(const Entry_ &b) {
        this->_id  = b._id;
        this->_idx = b._idx;

        return (*this);
      }

      virtual std::ostream& content(std::ostream& os) const  {
        return (os << _id << " Idx " << _idx );
      }
    };

  private:
  protected:
    Int_ _size, _dof;
    std::vector< Entry_ > _ids;

  public:
    Map_() : _size(0), _dof(0) { }
    Map_(Int_ size) : _size(size), _dof(0) { }
    Map_(const Map_ &b) : _size(b._size), _ids(b._ids), _dof(b._dof) { }

    virtual ~Map_() { 
      _ids.clear();
    }
    
    void set_size(Int_ size) { _size = size; }
    Int_ get_size() const { return _ids.size(); }

    Int_ dof() const { return _dof; }

    // Always reserve the size and do not go beyond
    void push_begin() {
      _ids.clear();
      _ids.reserve(_size);
      _dof = 0;
    }

    void push_begin(Int_ size) {
      set_size(size);
      push_begin();
    }

    void push(Id_ id) {
      // index is offset value in matrix, 
      // id can be int + dof,  (int,int) + dof
      _ids.push_back(Entry_
                     (id, (_ids.size() ? 
                           _ids.back().idx() + _ids.back().id().dof() : 0)));
      _dof += id.dof();
    }
    
    void push_end() {
      std::sort(_ids.begin(),_ids.end());
    }

    Bool_ find(Id_ &id, Int_ &loc) {
      if (_ids.size() && (loc < _ids.size())) {
        typename std::vector< Entry_ >::iterator low, begin = _ids.begin();
        begin += loc;

        low = std::lower_bound(begin, _ids.end(), Entry_(id, 0));
        if (low != _ids.end() && low->is_equal(id)) {
          loc = int(low - _ids.begin());
          return true;
        }
      }
      return false;
    }

    void unique() {
      _ids.resize(std::unique(_ids.begin(),_ids.end()) - _ids.begin());
      
      _dof = 0;
      for (Int_ i=0;i<_ids.size();++i) {
        _ids[i].idx() = _dof;
        _dof += _ids[i].id().get_dof();
      }
    }

    Id_& id(Int_ i) { return _ids[i].id(); } 
    Int_& idx(Int_ i) { return _ids[i].idx(); } 

    // This does not affect global dof
    void update_local_dof(Id_ &in) { 
      typename std::vector< Entry_ >::iterator low;
      low = std::lower_bound(_ids.begin(), _ids.end(), Entry_(in,0)); 
      
      Id_ &out = low->id();
      if (in == out) 
        out.set_dof( out.get_dof() + in.get_dof() );
    }

    void delete_local_dof() {
      for (Int_ i=0;i<_ids.size();++i)
        _ids[i].id().set_dof(0);
    }

    void clear() { _dof = 0; _size = 0; _ids.clear(); }

    Bool_ operator<(const Map_ &b) const {  
      if (this->get_size() != b.get_size()) 
        return ((this->get_size() - b.get_size()) < 0);

      for (Int_ i=0;i<this->get_size();++i)
        if (_ids[i] != b._ids[i]) 
          return (_ids[i] < b._ids[i]);
        
      return false;
    }
    Bool_ operator==(const Map_ &b) const { 
      if (this->get_size() != b.get_size())
        return false;

      for (Int_ i=0;i<this->get_size();++i)
        if (_ids[i] != b._ids[i]) 
          return false;
        
      return true;
    }

    Bool_ operator!=(const Map_ &b) const {
      return !((*this) == b);
    }

    virtual std::ostream& content(std::ostream& os) const  {
      os << "Map_ ( " << _ids.size() << " / " << _size << " )" 
         << ", Dof( " << _dof << " )" << std::endl;
      for (Int_ i=0;i<_ids.size();++i)
        os << _ids.at(i) << std::endl;
      return (os);
    }
  };


  // Mapper_Base_ is abstract class for polymophism.
  class Mapper_Base_ {
  public:
    Mapper_Base_() { }
    virtual~Mapper_Base_() { }
  };

  // Mapper_ relate the two in and out maps. 
  // Mapper_ is used for updating between Matrix Objects.
  template<class Id_>
  class Mapper_ : public Mapper_Base_,
                  public Disp_ {
  private:
  protected:
    Map_<Id_> *_in, *_out;
    std::vector<Int_> _idx;
  public:
    typedef Id_ _Id_;

    Mapper_() : _in(NULL), _out(NULL) { }
    Mapper_(Map_<Id_> *in, Map_<Id_> *out) : _in(in), _out(out) { }
    virtual~Mapper_() { _idx.clear(); }

    void set(Map_<Id_>* in, Map_<Id_>* out) { 
      _in = in; _out = out;
    }

    Int_ idx(Int_ i) { return _idx[i]; }
    Map_<Id_>* in() { return _in; }
    Map_<Id_>* out() { return _out; }

    
    void relate() {
      // assume that both Map_ in and out are sorted.
      _idx.clear();
      _idx.reserve(_in->get_size());

      Int_ loc=0;
      for (Int_ i=0;i<_in->get_size();++i) {
        Id_ v_in = _in->id(i);
        if (_out->find(v_in, loc))
          _idx.push_back(loc);
        else
          _idx.push_back(-1);
      }
    }
    
    
    /*
    void relate() {
      _idx.clear();
      _idx.reserve(_in->get_size());

      Int_ loc = 0;
      for (Int_ i=0;i<_in->get_size();++i) {
        Id_ v_in = _in->id(i);
        Bool_ flag = true;
        for (Int_ j=loc;j<_out->get_size();++j) {
          if (v_in == _out->id(j)) {
            loc = j; _idx.push_back(loc);
            flag = false;
            break;
          }
        }
        if (flag) {
          _idx.push_back(-1);
        }
      }
    }
    */

    void coarse(Map_<ID_Node_H_<1> > &in,
                Map_<ID_Node_H_<1> > &out) {
      if (!_idx.size()) 
        return;

      std::vector< std::pair<Int_,Int_> > v_out;
      v_out.reserve(_out->get_size());

      if (_idx.size() < 2 && _idx[0] >= 0) 
        v_out.push_back(std::make_pair(_idx[0],-1));

      for (Int_ i=0;i<(_idx.size()-1);++i) 
        if (_idx[i] >= 0 && (_idx[i+1] - _idx[i]) != 1) {
          v_out.push_back(std::make_pair(_idx[i],_idx[i+1]));
          //std::cout << "out = " << v_out.back().first 
          //          << ", " << v_out.back().second << "\n";
        }

      std::vector< std::pair<Int_,Int_> > minus_one;
      minus_one.reserve(_in->get_size());
      
      for (Int_ i=0;i<_idx.size();++i) {
        if (_idx[i] < 0) {
          Int_ now = i;
          for ( ;_idx[i]<0 && i<_idx.size(); ++i) ;
          minus_one.push_back(std::make_pair(now,i-now));
        }
      }

      ID_Node_H_<1> nd;      
      Int_ dof=0, c_out=0, c_minus=0, id=0;

      in.set_size(_in->get_size());
      out.set_size(_out->get_size());

      in.push_begin(); 
      out.push_begin();

      // This is necessary. little tricky to debug....
      Int_ prev=0, next=0;
      if (minus_one.size() && !minus_one[0].first) 
        prev = minus_one[0].second;

      if (prev < _idx.size() && _idx[prev] > 0) {
        dof = 0;
        for (Int_ i=0;i<_idx[prev];++i)
          dof += _out->id(i).dof();
        nd.set_dof(dof);
        nd.set_id(0,id++);
        out.push(nd);
      } 

      dof = 0;
      for (Int_ i=0;i<_idx.size();++i) {
        if ((i == minus_one[c_minus].first) && (c_minus < minus_one.size())) {
          // there is no match to out
          dof = 0;
          for (Int_ j=0;j<minus_one[c_minus].second;++j) 
            dof += _in->id(minus_one[c_minus].first+j).dof();

          nd.set_dof(dof);
          nd.set_id(0, id++);
          in.push(nd);
          i+=(minus_one[c_minus].second-1);
          
          prev = (minus_one[c_minus].first - 1);
          next = (minus_one[c_minus].first + minus_one[c_minus].second);

          if (prev >= 0 && next < _idx.size()) {
            dof =0;
            for (Int_ j=(_idx[prev]+1);j<_idx[next];++j)
              dof += _out->id(j).dof();
            if (dof) {
              nd.set_dof(dof);
              nd.set_id(0,id++);
              out.push(nd);
            }
          }
          dof =0;  ++c_minus;           
        } else {
          dof += _in->id(i).dof();
          if ((_idx[i] == v_out[c_out].first) && (c_out < v_out.size())) {
            nd.set_dof(dof);
            nd.set_id(0,id++); 
            in.push(nd); out.push(nd);
            
            if (v_out[c_out].second < 0) {
              // do nothing
            } else {
              nd.set_dof(_out->idx(v_out[c_out].second) -
                         _out->idx(v_out[c_out].first) -
                         _out->id(v_out[c_out].first).dof());
              nd.set_id(0,id++);
              out.push(nd);
            }
            dof = 0; ++c_out;
          } 
        }
      }

      // remainder to in
      dof = (_in->dof() - in.dof());
      if (dof) {
        nd.set_dof(dof);
        nd.set_id(0,id++);  
        in.push(nd); 
        out.push(nd);
      }

      // remainder to out
      dof = (_out->dof() - out.dof());
      if (dof) {
        nd.set_dof(dof);
        nd.set_id(0,id);
        out.push(nd);
      }
      out.push_end();
      in.push_end();
    }

    void clear() {
      _in = NULL; _out = NULL;
      _idx.clear();
    }

    virtual std::ostream& content(std::ostream& os) const  {
      os << "Mapper_ " << std::endl;
      os << "Map _in \n" << *_in ;

      os << "\n";
      os << "Relation  : ";
      for (Int_ i=0;i<_idx.size();++i)
        os << _idx.at(i) << "  ";
      os << "\n\n";

      os << "Map _out\n" << *_out;

      return os;
    }
  };
}

#endif
